#include "lpsolver.H"
#include <iomanip>
#include <fstream>

namespace LPSOLVER {
// functions for the matrix class

Matrix::
Matrix(size_t nrow, size_t ncolumn): // constructor     
    nrow(nrow), ncolumn(ncolumn) 
{
    data = new Real [nrow*ncolumn];
    for(std::size_t i=0; i<nrow*ncolumn; i++)
        data[i]=0;
}
Matrix::
~Matrix()
{
    delete [] data;
}       

Matrix::
Matrix(const Matrix &m)
{       
    this->nrow = m.nrow;
    this->ncolumn = m.ncolumn;
    data = new Real [nrow*ncolumn];
    for (size_t i=0; i<(nrow*ncolumn); i++) data[i] = m.data[i];
}


void Matrix::
print() const
{       
    for (size_t i=0; i<nrow; i++) {
        for (size_t j=0; j<ncolumn; j++) 
            std::cout << data[j+i*ncolumn] << "\t,";
        std::cout << std::endl;
    }
}
        
void Matrix::
setval(size_t i,size_t j,Real val) 
{
    data[i*ncolumn + j] = val;
}


//******Some useful functions for lpsolver**********

// calculates minimum value of an array pointer
Real min(const Real *data, const size_t size)
{       
    Real val = data[0];
    for (size_t i=1; i<size; i++) {
        if(val>data[i]) val=data[i];
    }
    return val;
}

// finds the arguments where minimum value occurs
size_t argmin(const Real *data,const size_t size)
{       
    size_t index = 0;
    for (size_t i=1; i<size; i++) {
        if(data[index]>data[i]) index=i;
    }
    return index;
}

void printvec(const size_t *vec, const size_t size)
{       
    for (size_t i=0; i<size; i++) std::cout << vec[i] << "\t";
    std::cout << std::endl;
}
        
void printvec(const Real *vec, const size_t size)
{       
    for (size_t i=0; i<size; i++) std::cout << vec[i] << "\t";
    std::cout << std::endl;
}

        
//<<<<<<Functions of the LpSolver class>>>>>>>>>

LpSolver::
LpSolver(const Matrix &Aeq, const Real *beq):
    n(Aeq.row()), m(Aeq.column()), table(m+1, n+1), track(2,n-1)
{       
    g = new Real [2*(m+n)];
    for(std::size_t i=0; i<2*(m+n); i++)
        g[i] = 0;
    td = new size_t [n];
    for(size_t i=0; i<n; i++)
        td[i] = 0;
    bd = new size_t [m+1];
    for(size_t i=0; i<(m+1); i++)
        bd[i] = 0;              
    gb = new Real [m+1];
    for(size_t i=0; i<(m+1); i++)
        gb[i] = 0;              
    mcost = new Real [2*(n-1)];
    for(size_t i=0; i<2*(n-1); i++)
        mcost[i] = 0;        
    for (size_t i = 0; i < m+1; i++) {
        for (size_t j=0; j < n+1; j++)
            table(i,j) = 0;
    }
    
    for (size_t i = 0; i < m; i++) {
        for (size_t j=0; j < n; j++)
            table(i,j) = Aeq(j,i);
    }

    table(m,n) = 1;
        
    // initializing g               
    for (size_t i = 0; i < n; i++) {
        g[i] = beq[i];
        g[i+n+m] = -1*beq[i];
    }
    // modifying simplex tableau
    for (size_t i = 0; i < m; i++) {
        if (table(i,0)>0) {
            bd[i] = i+n+(m+n)+1;
            for (size_t j = 0; j<n; j++)
                table(i,j) = table(i,j)*-1;
        }
        else bd[i] = i+n+1;
    }
    bd[m] = 1;

    for (size_t i = 0; i<n; i++) {
        for (size_t j = 0; j<m; j++)
            table(m,i) += (-1*table(j,i));
    }
    for (size_t i=0; i<n; i++) 
        td[i] = i+1;    
}

LpSolver::~LpSolver()
{
    delete [] g;
    delete [] gb;
    delete [] td;
    delete [] bd;
    delete [] mcost;
}                       

void LpSolver::gauss_jordan(const size_t row, const size_t column)
{       
    Real tmp_var;
    Real pivot = table(row,column);
    for (size_t j=0; j<n+1; j++) {
        table(row,j) /= pivot;
    }
        
    for (size_t i=0; i<m+1; i++) {
        tmp_var = table(i,column);
        for (size_t j=0; j<n+1; j++) {
            if (i!=row) 
                table(i,j) = table(i,j) - table(row,j)*tmp_var;
        }
    }
}

bool LpSolver::inEqc(const size_t i)
{       
    if (i<=n || (i>m+n && i<= (m+2*n))) 
        return 1;
    else 
        return 0;
}
        
bool LpSolver::in_bd(const size_t j)
{       
    for (size_t i=0; i<m+1; i++) {
        if (j==bd[i])
            return 1;
    }
    return 0;
}

bool LpSolver::in_track0(const size_t j)
{       
    for (size_t i=0; i<n-1; i++) {
        if (j==track(0,i))
            return 1;
    }
    return 0;
}
        
Real LpSolver::costCalc(size_t j)
{       
    Real cost = 0;
    for (size_t i=0; i<m+1; i++){
        cost += (g[bd[i]-1]*table(i,j));
    }
    return cost;
}
        
void LpSolver::margcost()
{
    z = costCalc(n);
    size_t j = 0;

    for (size_t i=0; i<n; i++) {
        if (!(in_bd(td[i]))) {
            track(0,j) = td[i];
            track(1,j) = i;
            mcost[j] = costCalc(i) -g[td[i]-1];
            if (inEqc(td[i]))
                mcost[j+n-1] = -1*mcost[j];
            else 
                mcost[j+n-1] = 2*z-1*mcost[j];
            j = j+1;
        }
    }
}

void LpSolver::getResult(Real *a)
{       
    size_t j;
    for (size_t i=0; i<m; i++) {
        if(in_bd(i+n+1)) 
            a[i] = -1*z;
        else if(in_bd(i+n+1+m+n)) 
            a[i] = z;
        else {
            if (in_track0(i+n+1)) {
                for (size_t k=0; k<n-1; k++) {
                    if(track(0,k)==i+n+1)
                        j = k;
                }
            }
            else if (in_track0(i+n+1+m+n)) {
                for (size_t k=0; k<n-1; k++) {
                    if (track(0,k)==i+n+1+m+n)
                        j = k+n-1;
                }
            }
            a[i] = mcost[j]-z;
        }
    }
    a[m] = z;
}

void LpSolver::solve(Real *a)
{       
    gauss_jordan(m,0);
    z = costCalc(n);

    // making objective function positive
    if (z<EPS) {
        for (size_t i=0; i<n; i++) 
            td[i] = td[i]+m+n;
        for (size_t i=0; i<m+1; i++) {
            if (bd[i]<=m+n) 
                bd[i] = bd[i]+m+n;
            else 
                bd[i] = bd[i]-m-n;
        }
    }

    margcost();
    size_t counter = 0; // limits the maximum no of iteration
    int row, column, argminimum, tdi;
    Real tmp;
    //Real abcd = min(mcost,2*(n-1));

    while (min(mcost,2*(n-1))<-EPS) {
        argminimum = argmin(mcost,2*(n-1));
                                
        if (argminimum > n-2) {
            column = track(1,argminimum-(n-1));
            tdi = track(0,argminimum-(n-1));
            if(inEqc(tdi)) {
                for (size_t i=0; i<m+1; i++)
                    table(i,column) = -1*table(i,column);
            }
            else {
                for (size_t i=0; i<m+1; i++)
                    table(i,column) = 2*table(i,n) - table(i,column);
            }
            if (tdi<=m+n) 
                td[column] = tdi+m+n;
            else 
                td[column] = tdi -m-n;
        }
        else 
            column = track(1,argminimum);

        row = -100;
        Real bigno = 1e+20;
        for (size_t i=0; i<m+1; i++){
            if (table(i,column) > EPS){
                tmp = table(i,n)/table(i,column);
                if (tmp<=bigno){
                    bigno = tmp; 
                    row = i;
                }
            }
        }

        if (row ==-100 || counter > 25) { // brakeing condition
            a[m] = INFI;
            return;                         
        }       
        for (size_t i=0; i<n; i++) {
            if (in_bd(td[i])) {
                td[i] = bd[row];
                for (size_t j=0; j<m+1; j++)
                    table(j,i)=0;
                table(row,i) = 1;
            }
        }
        bd[row] = td[column];           
        gauss_jordan(row,column);       
        margcost();
        counter++;
    }               
    getResult(a);                   
}

} // namespace ends

//***********************************************************
// For testing purpose
using namespace LPSOLVER; 
using namespace std;
int main(){
    size_t row, column;
    /*ifstream inFile;
      inFile.open("matrix");
      inFile >> row;
      inFile >> column;
      Matrix Aeq(row,column);
      for (size_t i=0; i<row; i++){
      for (size_t j=0; j<column; j++)
      inFile >> Aeq(i,j);
      }

      Real beq[column];
      for (size_t j=0; j<column; j++)
      inFile >> beq[j];

      inFile.close();
    */
    row =10; column = 11;
    Matrix Aeq(row,column);
    Real beq[10] = {-4.5,1.41421356237,-0.585786437627,-6.82842712475,-1.33226762955e-15,-2.0,4.82842712475,-6.0,-10.0,-18.0};
    Real t1[11] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
    Real t2[11] = {0.0,-1.41421356237,-1.41421356237,0.0,-1.41421356237,0.0,1.41421356237,0.0,0.0,-2.0,2.0};
    Real t3[11] = {0.0,1.41421356237,-1.41421356237,0.0,-1.41421356237,-2.82842712475,1.41421356237,2.0,0.0,2.0,0.0};
    Real t4[11] = {0.0,2.0,2.0,2.82842712475,2.0,-1.73185491414e-16,2.0,2.0,2.82842712475,-1.73185491414e-16,-2.0};
    Real t5[11] = {0.0,-2.0,2.0,0.0,2.0,-0.0,2.0,0.0,0.0,-4.0,0.0};
    Real t6[11] = {0.0,2.82842712475,-2.82842712475,0.0,-2.82842712475,4.89842541529e-16,2.82842712475,4.0,0.0,-3.46370982829e-16,-0.0};
    Real t7[11] = {0.0,-2.82842712475,-2.82842712475,0.0,-2.82842712475,-0.0,2.82842712475,0.0,0.0,3.46370982829e-16,-4.0};
    Real t8[11] = {0.0,2.0,2.0,0.0,2.0,0.0,2.0,0.0,0.0,4.0,4.0};
    Real t9[11] = {0.0,2.0,2.0,0.0,2.0,8.0,2.0,4.0,0.0,4.0,0.0};
    Real t10[11] = {0.0,4.0,4.0,8.0,4.0,2.99932144364e-32,4.0,4.0,8.0,2.99932144364e-32,4.0};
    for (int i=0; i<11; i++)
        {Aeq(0,i) = t1[i]; 
         Aeq(1,i) = t2[i];
         Aeq(2,i) = t3[i];
         Aeq(3,i) = t4[i];
         Aeq(4,i) = t5[i];
         Aeq(5,i) = t6[i];
         Aeq(6,i) = t7[i];
         Aeq(7,i) = t8[i];
         Aeq(8,i) = t9[i];
         Aeq(9,i) = t10[i];}
        
    Real *a;
    LpSolver *lp;
    for (int i=0; i<1; i++){
        lp = new LpSolver(Aeq, beq);
        a = new Real [column+1];
        for (size_t i=0; i<=column; i++)
            a[i]=0;
        lp->solve(a);   
        
        std::cout << "the answer is" << std::endl;
        printvec(a,column+1);
        delete lp;
        delete [] a;
    }
    return 0;       
}

        
        

